/*
GPS信息数据坐标系转换到ECEF坐标系

*/

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/nav_sat_status.hpp"
#include "basic_sprayer_interfaces/msg/nav_sta_dp.hpp"
#include "basic_sprayer_interfaces/msg/gps_to_ecef.hpp"

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>

using namespace std::chrono_literals;

bool flag = false;
// double lat_r=0;
// double lon_r=0;
// double alt_r=0;

double lat_r = std::numeric_limits<double>::quiet_NaN();
double lon_r = std::numeric_limits<double>::quiet_NaN();
double alt_r = std::numeric_limits<double>::quiet_NaN();
/*将 std::numeric_limits<double>::quiet_NaN() 赋值给参考点经纬度和高度的变量，表
示这些变量没有被初始化，即“不是一个数字（NaN）”。
这样做的好处是，如果在计算过程中使用了未初始化的变量，程序就会崩溃，并产生有用的错误信息*/

// double lat_r=30.557360;
// double lon_r=121.019622;
// double alt_r=5.003600;

class GpsSubscriber : public rclcpp::Node
{
public:
    GpsSubscriber()
        : Node("gps_subscriber")
    {
        subscription_gps = this->create_subscription<basic_sprayer_interfaces::msg::NavStaDP>(
            "dgps", 10, std::bind(&GpsSubscriber::dgps_callback, this, std::placeholders::_1));
        publisher_ecef = this->create_publisher<basic_sprayer_interfaces::msg::GpsToEcef>("ecef", 10);
    }

    void dgps_callback(const basic_sprayer_interfaces::msg::NavStaDP::SharedPtr msg)
    // 发布转换后的ECEF坐标
    {
        auto dgps_msg = basic_sprayer_interfaces::msg::NavStaDP();
        dgps_msg = *msg; // 获取当前点数据
        // rclcpp::Time t=->now();  //获取当前时间
        // int flag=0;
        //  double lat_r=0;
        //  double lon_r=0;
        //  double alt_r=0;

        if (!flag)
        {
            lat_r = dgps_msg.latitude;  // 参考点纬度
            lon_r = dgps_msg.longitude; // 参考点经度
            alt_r = dgps_msg.altitude;  // 参考点高度

            flag = true;
        }
        // 将GPS坐标转换为ECEF坐标   目标点
        double x, y, z; // tar_x  tar_y tar_z
        navSatFixToEnu(dgps_msg.latitude, dgps_msg.longitude, dgps_msg.altitude, &x, &y, &z);

        double ref_x, ref_y, ref_z; // 参考点
        calc_reference_coordinate(lat_r, lon_r, alt_r, &ref_x, &ref_y, &ref_z);

        // EcefToEnu
        double e, n, u;
        double e1, n1, u1;
        // calc_enu_coordinates(ref_x, ref_y, ref_z, x, y, z, e, n, u);
        double dx = x - ref_x;
        double dy = y - ref_y;
        double dz = z - ref_z;
        double slat = sin(lat_r * M_PI / 180.0);
        double clat = cos(lat_r * M_PI / 180.0);
        double slon = sin(lon_r * M_PI / 180.0);
        double clon = cos(lon_r * M_PI / 180.0);
        e1 = -slon * dx + clon * dy;
        n1 = -slat * clon * dx - slat * slon * dy + clat * dz;
        u1 = clat * clon * dx + clat * slon * dy + slat * dz;

        // ENU坐标系旋转到车身坐标系
        e = cos(dgps_msg.yaw) * e1 - sin(dgps_msg.yaw) * n1;
        n = sin(dgps_msg.yaw) * e1 + cos(dgps_msg.yaw) * n1;
        u = u1;

        // 偏差补偿offset
        double de = 0.3;
        double dn = 0.25;
        double du = 0;

        e = e + de;
        n = n + dn;
        u = u + du;

        // message消息格式
        auto message = basic_sprayer_interfaces::msg::GpsToEcef();
        message.x = e;
        message.y = n;
        message.z = u;
        RCLCPP_INFO(this->get_logger(), "longitude=%f,latitude=%f,altitude=%f", dgps_msg.longitude, dgps_msg.latitude, dgps_msg.altitude);
        RCLCPP_INFO(this->get_logger(), "e=%f,n=%f,u=%f", message.x, message.y, message.z);
        // 发布转换后的ECEF坐标
        publisher_ecef->publish(message);
    };

    // 计算目标点的地心坐标
    void navSatFixToEnu(double latitude, double longitude, double altitude, double *x, double *y, double *z)
    {
        // double latitude = latitude * M_PI / 180.0; // 使用 math.h 中的 pi 常量
        // double longitude = longitude * M_PI / 180.0;

        // 计算WGS84椭球体参数
        const double a = 6378137.0;
        const double f = 1.0 / 298.257223563;
        const double b = a * (1 - f);
        const double e = sqrt((a * a - b * b)) / a;
        const double N = a / sqrt(1 - e * e * sin(latitude * M_PI / 180.0) * sin(latitude * M_PI / 180.0));

        // 计算ECEF坐标
        *x = (N + altitude) * cos(latitude * M_PI / 180.0) * cos(longitude * M_PI / 180.0);
        *y = (N + altitude) * cos(latitude * M_PI / 180.0) * sin(longitude * M_PI / 180.0);
        *z = (N * (1 - e * e) + altitude) * sin(latitude * M_PI / 180.0);
    }

    // 计算参考点的地心坐标系直角坐标（需要提供参考点的经纬度）
    void calc_reference_coordinate(double ref_lat, double ref_lon, double ref_alt, double *ref_x, double *ref_y, double *ref_z)
    {
        const double a = 6378137.0;
        const double f = 1.0 / 298.257223563;
        const double b = a * (1 - f);
        const double e = sqrt((a * a - b * b)) / a;
        const double N = a / sqrt(1 - e * e * sin(ref_lat * M_PI / 180.0) * sin(ref_lat * M_PI / 180.0));
        *ref_x = (N + ref_alt) * cos(ref_lat * M_PI / 180.0) * cos(ref_lon * M_PI / 180.0);
        *ref_y = (N + ref_alt) * cos(ref_lat * M_PI / 180.0) * sin(ref_lon * M_PI / 180.0);
        *ref_z = (N * (1 - e * e) + ref_alt) * sin(ref_lat * M_PI / 180.0);
    }

private:
    rclcpp::Subscription<basic_sprayer_interfaces::msg::NavStaDP>::SharedPtr subscription_gps;
    rclcpp::Publisher<basic_sprayer_interfaces::msg::GpsToEcef>::SharedPtr publisher_ecef;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GpsSubscriber>());
    rclcpp::shutdown();
    return 0;
}
